// Math constants.
const float degreesPerRadian = 57.295779513082320876798154814105;

// Sensor constants.
const float sensorPowerSupply = 3.3;
const float accSensitivity = 0.33;
const float gyroSensitivity = 0.00333;

float rawToRate(int rawValue, float zeroRate) {
  return (rawValue - zeroRate) * sensorPowerSupply
    / (gyroSensitivity * 1023);
}

float rawToGravity(int rawValue, float zeroBias) {
  return (rawValue - zeroBias) * sensorPowerSupply
    / (accSensitivity * 1023);
}

float getMagnitude(float x, float y, float z) {
  return sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2));
}

float normalizeValues(float * x, float * y, float * z,
float * mag) {
  *mag = *mag / *mag;
  *x = *x / * mag;
  *y = *y / * mag;
  *z = *z / * mag;
}

void getTrueAngles(float * xA, float * yA, float * xV,
float * yV, float *zV) {
  if(*zV < 0) {
    if (*zV <= 0) {
      *xA = 360 - *xA;
      *yA = 360 - *yA;
    }
  }
}

float radianToDegree(float radian) {
  return radian * degreesPerRadian;
}

void getTrueAcc(float * cX, float * cY, float xV,
float yV, float xA, float yA) {
  *cX = xV - 1 * cos(xA / degreesPerRadian);
  *cY = yV - 1 * cos(yA / degreesPerRadian);
}

